#ifndef DSO_ENERGY_FUNCTIONAL_H
#define DSO_ENERGY_FUNCTIONAL_H

#include "dso/util/NumType.h"
#include "dso/util/IndexThreadReduce.h"

#include <vector>
#include <cmath>
#include <map>

// 这个东西是类似于Optimizer的玩意
// 严格说是一个sliding window形式的，带marg和fej的，基于光度误差的优化
// 如果不关心原理的话，那么insertResidual, insertFrame, insertPoint，然后solveSystemF
// 如果关心原理的话，我做了一些注释。其他用到的结构见EnegyFunctoinalStructs.h

// F后缀是指float，应该是engel写了一个double版本的。

namespace dso
{
class PointFrameResidual;
class CalibHessian;
class FrameHessian;
class PointHessian;


class EFResidual;
class EFPoint;
class EFFrame;
class EnergyFunctional;
class AccumulatedTopHessian;
class AccumulatedTopHessianSSE;
class AccumulatedSCHessian;
class AccumulatedSCHessianSSE;

extern bool EFAdjointsValid;    // setAdjointsF后，将此置真
extern bool EFIndicesValid;     // makeIDX 后，将此置真
extern bool EFDeltaValid;

class EnergyFunctional
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    // 和各类都是Friend，可以随意调用它们的数据
    friend class EFFrame;
    friend class EFPoint;
    friend class EFResidual;
    friend class AccumulatedTopHessian;
    friend class AccumulatedTopHessianSSE;
    friend class AccumulatedSCHessian;
    friend class AccumulatedSCHessianSSE;

    EnergyFunctional();
    
    ~EnergyFunctional();

    // 添加Frame-Point的残差
    EFResidual* insertResidual ( PointFrameResidual* r );
    
    // 添加关键帧
    EFFrame* insertFrame ( FrameHessian* fh, CalibHessian* Hcalib );
    
    // 添加点
    EFPoint* insertPoint ( PointHessian* ph );
    
    // 求解整个系统
    void solveSystemF ( int iteration, double lambda, CalibHessian* HCalib );

    // 扔掉一个残差
    void dropResidual ( EFResidual* r );
    
    // 边缘化掉一个Frame
    void marginalizeFrame ( EFFrame* fh );
    
    // 扔掉一个点
    void removePoint ( EFPoint* ph );

    // 边缘化一个点
    void marginalizePointsF();
    
    // 扔掉一个点
    void dropPointsF();
    
    // 计算energy
    double calcMEnergyF();
    
    // 多线程计算energy
    double calcLEnergyF_MT();

    // 设置每个Residuals的index, 并添加每个frame关联的point
    void makeIDX();
    
    void setDeltaF ( CalibHessian* HCalib );
    
    // 设置 Frame 到 Frame 之间的伴随
    void setAdjointsF ( CalibHessian* Hcalib );

    std::vector<EFFrame*> frames; // 所有的EFFrame
    int nPoints, nFrames, nResiduals; // 点/帧/残差的数量              

    MatXX HM;   // H 
    VecX bM;    // b

    int resInA, resInL, resInM;
    MatXX lastHS;
    VecX lastbS;
    VecX lastX;
    std::vector<VecX> lastNullspaces_forLogging;
    std::vector<VecX> lastNullspaces_pose;
    std::vector<VecX> lastNullspaces_scale;
    std::vector<VecX> lastNullspaces_affA;
    std::vector<VecX> lastNullspaces_affB;

    IndexThreadReduce<Vec10>* red;
    /// 连通图
    /// index高32位为host id, 低32位为target的id
    std::map<uint64_t,
        Eigen::Vector2i,
        std::less<uint64_t>,
        Eigen::aligned_allocator<std::pair<uint64_t, Eigen::Vector2i>>
        > connectivityMap;

private:
    VecX getStitchedDeltaF() const;

    void resubstituteF_MT ( VecX x, CalibHessian* HCalib, bool MT );
    void resubstituteFPt ( const VecCf &xc, Mat18f* xAd, int min, int max, Vec10* stats, int tid );

    void accumulateAF_MT ( MatXX &H, VecX &b, bool MT );
    void accumulateLF_MT ( MatXX &H, VecX &b, bool MT );
    void accumulateSCF_MT ( MatXX &H, VecX &b, bool MT );

    void calcLEnergyPt ( int min, int max, Vec10* stats, int tid );

    void orthogonalize ( VecX* b, MatXX* H );
    
    // adjoint相关
    Mat18f* adHTdeltaF;
    Mat88* adHost;      // host那边 nFrame个8x8的矩阵
    Mat88* adTarget;    // target那边 nFrame个8x8的矩阵

    // float形式
    Mat88f* adHostF;
    Mat88f* adTargetF;

    VecC cPrior;
    VecCf cDeltaF;
    VecCf cPriorF;

    AccumulatedTopHessianSSE* accSSE_top_L;
    AccumulatedTopHessianSSE* accSSE_top_A;
    AccumulatedSCHessianSSE* accSSE_bot;

    std::vector<EFPoint*> allPoints;
    std::vector<EFPoint*> allPointsToMarg;
    float currentLambda;
};

}


#endif
